#! /usr/bin/env python3
'''
author:toamin
function:代码由四部分组成,定位信息+规划算法+控制算法+Ros通信,实现单车点到点规划控制
'''
import sys
sys.path.append('/home/mvccep/MVTP_ws_tm/src/one_car_planning_control/base/')
from mvccep_image_localization.msg import base_localization # 获取定位信息
from geometry_msgs.msg import Twist
import rospy # ros发布订阅用到
import math
import startP_sub
from LQR import LQRControl
from pathPreProcess import pathPreProcess
from HAstar import hybridAstarPathPlanning
from drawPath import drawPath
import csv
import matplotlib.pyplot as plt


# 车辆状态量
class State:
    def __init__(self, x, y, yaw, v):
        self.x = x
        self.y = y
        self.yaw = yaw # yaw单位为角度
        self.v = v # 小车当前速度

# ros通信
class OneCarControl(object):
    """
    path已知,由控制算法得到车辆控制量,随后Ros进行发布
    """
    def __init__(self, path, robot_id):
        self.robot_id = robot_id
        self.path = path
        # 定义订阅者，订阅位置信息，Subscribe类的实例化（话题名， 消息类型， 回调函数）
        self.draw_state = State([],[],[],[])
        self.sub = rospy.Subscriber("base_localization", base_localization, self.controlCallback, queue_size = 30)
        # 定义发布者
        self.pub = rospy.Publisher("/robot_{}/cmd_vel".format(robot_id), Twist, queue_size = 30)
        self.lqrcontroller = LQRControl(self.path)
        self.traj = self.lqrcontroller.traj
        self.Point_O = self.lqrcontroller.Point_O
        
    # 回调函数是系统进行传参的，以下msg接收的值是话题/base_localization发布出来的消息
    # (相关命令 rostopic echo /base_localization)
    def controlCallback(self, msg):
        # 从定位拿到小车实时状态
        self.robot_list = [[msg.robot_0.pos.x, msg.robot_0.pos.y, msg.robot_0.angle, msg.robot_0.twist_linear.x], 
                           [msg.robot_1.pos.x, msg.robot_1.pos.y, msg.robot_1.angle, msg.robot_1.twist_linear.x],
                           [msg.robot_2.pos.x, msg.robot_2.pos.y, msg.robot_2.angle, msg.robot_2.twist_linear.x],
                           [msg.robot_3.pos.x, msg.robot_3.pos.y, msg.robot_3.angle, msg.robot_3.twist_linear.x],
                           [msg.robot_4.pos.x, msg.robot_4.pos.y, msg.robot_4.angle, msg.robot_4.twist_linear.x]]
        self.state = State(self.robot_list[self.robot_id][0], self.robot_list[self.robot_id][1], 
                           self.robot_list[self.robot_id][2], self.robot_list[self.robot_id][3])
        
        self.draw_state.x.append(self.state.x)
        self.draw_state.y.append(self.state.y)
        self.draw_state.yaw.append(self.state.yaw)
        self.draw_state.v.append(self.state.v) # 画图用

        if not self.lqrcontroller.STOP_FLAG:
            # 纯跟踪算法，传入小车实时状态、两个数组(即一系列路径点的二维坐标)，得到每个点对应的方向盘转角
            vel_msg = Twist() # 定义一条Twist类型的消息
            vel_msg.linear.x, vel_msg.angular.z = self.lqrcontroller.lqr_speed_steering_control(self.state) # 整段控制     
            # 发布线速度加速度信息
            self.pub.publish(vel_msg)
            # rospy.loginfo("Publish velocity command [%.2f m/s, %.2f rad/s]", vel_msg.linear.x, vel_msg.angular.z)                 
        else:
            # 自动结束ros发布的命令
            rospy.loginfo("*** Arrived the end point! ---------->>")
            rospy.signal_shutdown('Over')
            

def getParam():
    try:
        rospy.get_param('~Flag')
        robot_id = rospy.get_param('~robot_id')
        endP = rospy.get_param('~endP') # 运行的时候通过命令行给定
        endP = endP.split(",")
        endP = [float(endP[0]), float(endP[1]), round(math.radians(float(endP[2])), 2)]
        # 设置endP格式:输入[0.52,0.52,90] -> 转化为[0.52,0.52,1.57] (转成弧度并保留两位小数)
    except KeyError:
        robot_id = 3
        # endP = [0.6, -0.52, math.radians(-90)] # 调试的时候给固定值
        angle = 90
        endP = [0.52, -0.52, math.radians(angle)] # 调试的时候给固定值
        # endP = [0.52, -0.52, math.radians(angle)] # 调试的时候给固定值
    except Exception:
        print("请正确输入参数，例如:roslaunch one_car_planning_control one_car_planning_control endP:=0,-0.52,90")
    return robot_id, endP


if __name__ == '__main__':
    try:
        print("*** 开始执行主程序--------------->>\n")
        # 初始化一个节点，anonymous表示是否在节点名后追加随机数以确保节点名的唯一性
        rospy.init_node("planning_control_node", anonymous=False)
        # 获取参数
        robot_id, endP = getParam() # 运行的时候通过命令行给定
        startP = startP_sub.startP_sub(robot_id)# 从定位获取车辆的起始点（当前点）
        print("*** 被控车辆: ", robot_id, "号\n", "*** 车辆起点: ", startP, "\n*** 车辆终点: ", endP, "\n")
        # A*规划路径
        path = hybridAstarPathPlanning(startP, endP) # 调用混合A*算法，传入起点与终点后规划出轨迹并return

        # """给定轨迹测试LQR"""
        # robot_id = 3
        # class Path:
        #     def __init__(self, x, y, yaw, direction, cost = 0):
        #         self.x = x
        #         self.y = y
        #         self.yaw = yaw
        #         self.direction = direction
        # path_list = []
        # with open('path.csv') as points_file:
        #     reader = csv.reader(points_file)
        #     for row in reader:
        #         path_list.append([float(row[0]), float(row[1]), float(row[2])])
        # x = [path_list_x[0] for path_list_x in path_list]
        # y = [path_list_y[1] for path_list_y in path_list]
        # yaw = [path_list_yaw[2] for path_list_yaw in path_list]
        # direction = [1] * len(x)
        # path = Path(x, y, yaw, direction)
        
        # flag = 0 # 倒车判据
        # if flag:
        #     # 反转path(倒车)
        #     path.x, path.y = path.x[::-1], path.y[::-1]
        #     path.direction = [i * -1 for i in path.direction]
        
        # plt.plot(path.x, path.y)
        # plt.plot(path.x, path.yaw)
        # plt.show()

        # 构建控制器
        trajController = OneCarControl(path, robot_id)
        rospy.spin()

        drawPath(trajController.traj, trajController.draw_state, trajController.Point_O) # 画预处理散点图
        
    except rospy.ROSInterruptException:
        rospy.signal_shutdown("err")

# https://chat1.binjie.site:7777/#/chat/1680680039620
# 中文版ChatGPT